/**
 * @file dev_mpu6050.c
 * @brief 
 * 
 * @author dalin (dalin@robot.com)
 * @version 1.0
 * @date 2023-06-03
 * 
 * @copyright Copyright (c) 2023  OpenSir Tech.co, Ltd
 * 
 * @par 修改日志:
 * Date           Version     Author      Description
 * 2023-06-03     1.0         yangsl      touch
 */

#include "dev_mpu6050.h"

#include "filter.h"
#include "math.h"
#include "mlog.h"

#define MPU6050_DEV		DRV_I2C1
#define MPU6050_ADDR	0xD0
mpu6050Data_t 	_mpu;


static int8_t mpu6050_i2c_read(uint16_t mem_addr, uint8_t *data, uint8_t len)
{
	i2cDrv_t mpu6050_dev;
	
	mpu6050_dev.hi2c = MPU6050_DEV;
	mpu6050_dev.dev_addr = MPU6050_ADDR;
	mpu6050_dev.mem_addr = mem_addr;
	mpu6050_dev.mem_size = DRV_I2C_MEMSIZE_16BIT;
	mpu6050_dev.timeout  = 1000;

	drv_i2c_recv(mpu6050_dev, data, (uint8_t)len);
}

static int8_t mpu6050_i2c_write(uint16_t mem_addr, uint8_t *data, uint8_t len)
{
	i2cDrv_t mpu6050_dev;
	
	mpu6050_dev.hi2c = MPU6050_DEV;
	mpu6050_dev.dev_addr = MPU6050_ADDR;
	mpu6050_dev.mem_addr = mem_addr;
	mpu6050_dev.mem_size = DRV_I2C_MEMSIZE_16BIT;
	mpu6050_dev.timeout  = 1000;

	drv_i2c_send(mpu6050_dev, data, (uint8_t)len);
}


/**
 * @brief 初始化MOU6050配置
 * 
 * 
 * @return int8_t 
 */
int8_t mpu6050_open(void)
{
	uint8_t check;
	mpu6050_i2c_read(WHO_AM_I,&check, 1);

	/* 确认设备用 地址寄存器 */
	if(check == 0x68) 
	{	
		log_info("mpu6050 id: %x", check);
		/* 唤醒 */
		check = 0x00;
		mpu6050_i2c_write(PWR_MGMT_1,&check, 1); 	    

		/* 1Khz的速率 */
		check = 0x00;	
		mpu6050_i2c_write(SMPLRT_DIV,&check, 1);	    

		/* 内部低通 */
		check = 0x03;
		mpu6050_i2c_write(CONFIG, &check, 1);	 	

		/* 加速度配置 */
		check = 0x10;
		mpu6050_i2c_write(ACCEL_CONFIG,&check, 1);	 	

		/* 陀螺配置 */
		check = 0x18;
		mpu6050_i2c_write(GYRO_CONFIG,&check, 1);		
		return DEV_EOK;
	}
	else
	{
		log_error("mpu6050 dectected error");
		return DEV_ERROR;
	}
}

/******************************************mpu read******************************************/
/**
 * @brief 读取MPU6050的三轴加速度原始数据
 * 
 * @param  data             : 三轴加速度数据指针变量
 * 
 */
void mpu6050_read_accel(void *data)
{
	mpu_xyz_t *pdata = (mpu_xyz_t *)data;
	uint8_t read_buf[6];
	
	/* 寄存器依次是加速度X高 - 加速度X低 - 加速度Y高位 - 加速度Y低位 - 加速度Z高位 - 加速度度Z低位 */											
	mpu6050_i2c_read(ACCEL_XOUT_H, read_buf, 6); 
	
	pdata->_x = (int16_t)(read_buf[0] << 8 | read_buf[1]);
	pdata->_y = (int16_t)(read_buf[2] << 8 | read_buf[3]);
	pdata->_z = (int16_t)(read_buf[4] << 8 | read_buf[5]);
	
	
}

/**
 * @brief 读取MPU6050的三轴角加速度原始数据
 * 
 * @param  data             : 三轴加速度数据指针变量
 * 
 */
void mpu6050_read_gyro(void *data)
{
	mpu_xyz_t *pdata = (mpu_xyz_t *)data;
	uint8_t read_buf[6];

	/* 寄存器依次是角度X高 - 角度X低 - 角度Y高位 - 角度Y低位 - 角度Z高位 - 角度Z低位 */
	mpu6050_i2c_read(GYRO_XOUT_H, read_buf, 6); 
	
	pdata->_x = (int16_t)(read_buf[0] << 8 | read_buf[1]);
	pdata->_y = (int16_t)(read_buf[2] << 8 | read_buf[3]);
	pdata->_z = (int16_t)(read_buf[4] << 8 | read_buf[5]);
	
}

/**
 * @brief 读取陀螺仪温度信息
 * 
 * @param  data             : 温度存储地址
 * 
 */
void mpu6050_read_temp(void *data)
{
	float *pdata = data;
    uint8_t read_buf[2];
	
	mpu6050_i2c_read(TEMP_OUT_H, read_buf, 2); 
	
	_mpu.temp = (int16_t)(read_buf[0] << 8 | read_buf[1]);
	
	*pdata = 36.53f + (_mpu.temp / 340.0f);
}
/****************************************** 数据滤波 ******************************************/

/**
 * @brief 读取MPU6050的三轴加速度原始数据
 * 
 * @param  data             : 输出校准&滤波后的数据
 */
static void get_acc_raw(mpu_xyz_t *data)
{
	mpu_xyz_t acc_raw;
	mpu6050_read_accel(&acc_raw);

	memcpy(data, &acc_raw, sizeof(mpu_xyz_t));
//	_mpu.att_acc_factor = 0.01f /( 0.01f + 1 / (2.0f * PI * 25) );
//	iir_lpf_run((f_xyz_t *)&acc_raw, (f_xyz_t *)data, _mpu.att_acc_factor);      //姿态解算时加速度低通滤波
}

/**
  * @brief   巴特沃斯低通滤波器参数（200hz采样率，30hz截止频率）
  */
butterworthParameter_t gyro_30hz_parameter =
{
  //200hz---30hz
  1, 				 -0.7477891782585,    	0.272214937925,
  0.1311064399166,   0.2622128798333,   	0.1311064399166 
}; 

/**
 * @brief 读取陀螺仪三轴原始数据 & 零偏校准去除 & 低通滤波
 * 
 * @param  data             : 输出校准&滤波后的数据
 * @param  offset           : 校准值
 */
static void get_gyro_raw(mpu_xyz_t *data, mpu_xyz_t *offset)
{
	mpu_xyz_t gyro_raw;
	mpu6050_read_gyro(&gyro_raw);

	/* 原始数据减去校准零偏数据 */
	gyro_raw._x = (float)gyro_raw._x - offset->_x;
	gyro_raw._y = (float)gyro_raw._y - offset->_y;
	gyro_raw._z = (float)gyro_raw._z - offset->_z;     

	/* 三轴角速度原始数据进行滤波处理并转为浮点类型 */
	butterworth_lpf_run((f_xyz_t *)&gyro_raw , (f_xyz_t *)data , gyro_30hz_parameter);  
}

/****************************************** 数据转换 ******************************************/
/**
 * @brief （滤波后）原始加速度转为重力加速度g为单位数据
 * 
 * @param  acc_in           : 原始加速度浮点类型指针变量
 * @param  acc_out          : 以g为单位的加速度浮点数据指针变量
 * 
 */
static void get_acc_g(mpu_xyz_t *acc_in,mpu_xyz_t *acc_out)
{
	acc_out->_x = (float)(acc_in->_x * acc_raw_to_g);
	acc_out->_y = (float)(acc_in->_y * acc_raw_to_g);
	acc_out->_z = (float)(acc_in->_z * acc_raw_to_g);
}

/**
 * @brief （滤波后）原始陀螺仪数据转为弧度/秒为单位的数据
 * 
 * @param  gyro_in          : 原始的陀螺仪浮点数据指针变量
 * @param  gyro_out         : 以rad/s为单位的陀螺仪浮点数据指针变量
 * 
 */
static void get_rad_s(mpu_xyz_t *gyro_in,mpu_xyz_t *gyro_out)
{ 
	gyro_out->_x = (float)(gyro_in->_x * gyro_raw_to_radian_s);
	gyro_out->_y = (float)(gyro_in->_y * gyro_raw_to_radian_s);
	gyro_out->_z = (float)(gyro_in->_z * gyro_raw_to_radian_s);
}

/**
 * @brief （滤波后）原始陀螺仪数据转为度/秒为单位的数据
 * 
 * @param  gyro_in          : 原始的陀螺仪浮点数据指针变量
 * @param  gyro_deg_out     : 以deg/s为单位的陀螺仪浮点数据指针变量
 * 
 */
static void get_deg_s(mpu_xyz_t *gyro_in,mpu_xyz_t *gyro_deg_out)
{
	gyro_deg_out->_x = (float)(gyro_in->_x * gyro_raw_to_deg_s);
	gyro_deg_out->_y = (float)(gyro_in->_y * gyro_raw_to_deg_s);
	gyro_deg_out->_z = (float)(gyro_in->_z * gyro_raw_to_deg_s);    
}

/****************************************** 姿态计算 ******************************************/
_Matrix Mat = {0};
// _Attitude att = {0};
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        //quaternion elements representing theestimated orientation


/*
 * 函数名：mahony_update
 * 描述  ：四元素姿态解算
 * 输入  ：陀螺仪三轴数据（单位：弧度/秒），加速度三轴数据（单位：g）    
 * 返回  ：     
 */
//Gyroscope units are radians/second, accelerometer  units are irrelevant as the vector is normalised.
void mahony_update(mpu_xyz_t *gyro, mpu_xyz_t *acc, imuAngle_t *angle) 
{

#define MahonyPERIOD        10.0f            //姿态解算周期（ms）
#define kp 	    			6.0f      	 	//proportional gain governs rate of convergence to accelerometer/magnetometer 
#define ki 	    			0.0005f    		//integral gain governs rate of convergenceof gyroscope biases
#define KV   				1.0				// gyro和acc权重

    static float exInt = 0, eyInt = 0, ezInt = 0;       //scaled integral error  
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;
	float gx, gy,  gz;
	float ax, ay,  az;
	
	gx = gyro->_x * KV;
	gy = gyro->_y * KV;
	gz = gyro->_z * KV;
	ax = acc->_x;
	ay = acc->_y;
	az = acc->_z;

	if(ax*ay*az==0)
		return;

	//[ax,ay,az]是机体坐标系下加速度计测得的重力向量(竖直向下)
	norm = inv_sqrt(ax*ax + ay*ay + az*az);
	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;

	//VectorA = MatrixC * VectorB
	//VectorA ：参考重力向量转到在机体下的值
	//MatrixC ：地理坐标系转机体坐标系的旋转矩阵  
	//VectorB ：参考重力向量（0,0,1）      
	//[vx,vy,vz]是地理坐标系重力分向量[0,0,1]经过DCM旋转矩阵(C(n->b))计算得到的机体坐标系中的重力向量(竖直向下)    

	vx = Mat.DCM_T[0][2];
	vy = Mat.DCM_T[1][2];
	vz = Mat.DCM_T[2][2];

	//机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’
	//利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
	//实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
	//但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
	//两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
	//加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
	//也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。

	ex = ay*vz - az*vy;
	ey = az*vx - ax*vz;
	ez = ax*vy - ay*vx;

	//对误差向量进行积分
	exInt = exInt + ex*ki;
	eyInt = eyInt + ey*ki;
	ezInt = ezInt + ez*ki;

	//姿态误差补偿到角速度上，修正角速度积分漂移，通过调节Kp、Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
	gx = gx + kp*ex + exInt;
	gy = gy + kp*ey + eyInt;
	gz = gz + kp*ez + ezInt;

	//一阶龙格库塔法更新四元数 
	q0 = q0 + (-q1*gx - q2*gy - q3*gz)* MahonyPERIOD * 0.0005f;
	q1 = q1 + ( q0*gx + q2*gz - q3*gy)* MahonyPERIOD * 0.0005f;
	q2 = q2 + ( q0*gy - q1*gz + q3*gx)* MahonyPERIOD * 0.0005f;
	q3 = q3 + ( q0*gz + q1*gy - q2*gx)* MahonyPERIOD * 0.0005f; 

	//把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
	norm = inv_sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 * norm;
	q1 = q1 * norm;
	q2 = q2 * norm;
	q3 = q3 * norm;

	//四元素转欧拉角
	angle->roll  =  atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * rad_to_angle;
	angle->pitch =  asin(2.0f*(q0*q2 - q1*q3)) * rad_to_angle;       

	//z轴角速度积分的偏航角
	// att.yaw += Mpu.deg_s.z  * MahonyPERIOD * 0.001f;   
}

/****************************************** 坐标轴变换 ******************************************/
/*
 * 函数名：rotation_matrix
 * 描述  ：旋转矩阵：机体坐标系 -> 地理坐标系
 * 输入  ： 
 * 返回  ：     
 */
void rotation_matrix(void)
{
  Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
  Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
  Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);

  Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
  Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
  Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);
   
  Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
  Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
  Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;   
}
/*
 * 函数名：rotation_matrix_T
 * 描述  ：旋转矩阵的转置矩阵：地理坐标系 -> 机体坐标系
 * 输入  ： 
 * 返回  ：     
 */
void rotation_matrix_T(void)
{
  Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
  Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);    
  Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2); 
  
  Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
  Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;  
  Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);    
  
  Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
  Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
  Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;   
}
/*
 * 函数名：Matrix_ready
 * 描述  ：矩阵更新准备，为姿态解算使用
 * 输入  ： 
 * 返回  ：     
 */
void matrix_ready(void)
{
  rotation_matrix();                      //旋转矩阵更新
  rotation_matrix_T();                    //旋转矩阵的逆矩阵更新
}


/****************************************** 计算角度 ******************************************/
void get_imu_angle(void * data)
{
	mpu_xyz_t gyro_raw, acc_raw;
	mpu_xyz_t gyro_raw_s, acc_raw_g;
	imuAngle_t *angle = (imuAngle_t *)data;

	get_acc_raw(&acc_raw);
	get_gyro_raw(&gyro_raw , &_mpu.accel_offset);

	get_rad_s(&gyro_raw, 	&gyro_raw_s);
	get_acc_g(&acc_raw, 	&acc_raw_g);

	mahony_update(&gyro_raw_s, &acc_raw_g, angle);

	matrix_ready();
}
